#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
机器人模型模块

此模块提供用于路径规划的机器人类
"""

import numpy as np


class Robot:
    """
    表示用于路径规划的简单机器人模型的类。
    对于RRT实现，机器人被建模为一个圆形。
    """
    
    def __init__(self, radius, step_size):
        """
        使用给定参数初始化机器人模型。
        
        参数:
            radius (float): 机器人半径
            step_size (float): 运动规划的最大步长
        """
        self.radius = radius
        self.step_size = step_size
    
    def compute_next_state(self, current_state, target_state):
        """
        计算给定当前状态和目标状态的下一个状态，
        同时遵守最大步长限制。
        
        参数:
            current_state (tuple): 当前(x, y)位置
            target_state (tuple): 目标(x, y)位置
            
        返回:
            tuple: 迈出一步后的新(x, y)位置
        """
        # 提取坐标
        current_x, current_y = current_state
        target_x, target_y = target_state
        
        # 计算方向向量
        dx = target_x - current_x
        dy = target_y - current_y
        
        # 计算距离
        distance = np.sqrt(dx**2 + dy**2)
        
        # 如果距离小于步长，直接返回目标
        if distance <= self.step_size:
            return target_state
        
        # 否则，朝目标方向迈出一个step_size大小的步伐
        scale = self.step_size / distance
        new_x = current_x + dx * scale
        new_y = current_y + dy * scale
        
        return new_x, new_y
    
    def steer(self, from_state, to_state, step_size=None):
        """
        引导机器人从一个状态朝向另一个状态，可能使用自定义步长。
        
        参数:
            from_state (tuple): 起始(x, y)位置
            to_state (tuple): 目标(x, y)位置
            step_size (float, optional): 自定义步长，默认为机器人的step_size
            
        返回:
            tuple: 引导后的新(x, y)位置
        """
        if step_size is None:
            step_size = self.step_size
            
        # 提取坐标
        from_x, from_y = from_state
        to_x, to_y = to_state
        
        # 计算方向向量
        dx = to_x - from_x
        dy = to_y - from_y
        
        # 计算距离
        distance = np.sqrt(dx**2 + dy**2)
        
        # 如果距离小于步长，直接返回目标
        if distance <= step_size:
            return to_state
        
        # 否则，朝目标方向迈出一个step_size大小的步伐
        scale = step_size / distance
        new_x = from_x + dx * scale
        new_y = from_y + dy * scale
        
        return new_x, new_y 